// Frustum.h
//
#ifndef FRUSTUM_H
#define FRUSTUM_H

#include "OpenGL.h"

#include "IDSReal3D.h"
#include "IDSMatrix3D.h"
#include "IDSMatrix.h"

namespace IDS{
namespace OpenGL{

class Frustum
{
private:
	int viewport[2];
	IDSReal3D viewerPos;

	float frustum[6][4]; //planes

public:
	enum TestResult{INSIDE, OUTSIDE, INTERSECT};

	double nearClip;
	double farClip;

	// -------------------------------------------------------------------------------------------------
	Frustum()  {nearClip = 0; farClip = 0;}
	
	// -------------------------------------------------------------------------------------------------
	~Frustum() {}

	// -------------------------------------------------------------------------------------------------
	void compute(int vpWidth, int vpHeight)
	{
		float   proj[16];
		float   modl[16];
		float   clip[16];
		float   t;

		viewport[0] = vpWidth;
		viewport[1] = vpHeight;

		// ClipMatrix = PRJ * MV
		glGetFloatv( GL_PROJECTION_MATRIX, proj );
		glGetFloatv( GL_MODELVIEW_MATRIX,  modl );

		clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
		clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
		clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
		clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];

		clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
		clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
		clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
		clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];

		clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
		clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
		clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
		clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];

		clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
		clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
		clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
		clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];

		/* Extract the numbers for the RIGHT plane */
		frustum[0][0] = clip[ 3] - clip[ 0];
		frustum[0][1] = clip[ 7] - clip[ 4];
		frustum[0][2] = clip[11] - clip[ 8];
		frustum[0][3] = clip[15] - clip[12];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[0][0] * frustum[0][0] + frustum[0][1] * frustum[0][1] + frustum[0][2] * frustum[0][2] );
		frustum[0][0] *= t;
		frustum[0][1] *= t;
		frustum[0][2] *= t;
		frustum[0][3] *= t;

		/* Extract the numbers for the LEFT plane */
		frustum[1][0] = clip[ 3] + clip[ 0];
		frustum[1][1] = clip[ 7] + clip[ 4];
		frustum[1][2] = clip[11] + clip[ 8];
		frustum[1][3] = clip[15] + clip[12];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[1][0] * frustum[1][0] + frustum[1][1] * frustum[1][1] + frustum[1][2] * frustum[1][2] );
		frustum[1][0] *= t;
		frustum[1][1] *= t;
		frustum[1][2] *= t;
		frustum[1][3] *= t;

		/* Extract the BOTTOM plane */
		frustum[2][0] = clip[ 3] + clip[ 1];
		frustum[2][1] = clip[ 7] + clip[ 5];
		frustum[2][2] = clip[11] + clip[ 9];
		frustum[2][3] = clip[15] + clip[13];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[2][0] * frustum[2][0] + frustum[2][1] * frustum[2][1] + frustum[2][2] * frustum[2][2] );
		frustum[2][0] *= t;
		frustum[2][1] *= t;
		frustum[2][2] *= t;
		frustum[2][3] *= t;

		/* Extract the TOP plane */
		frustum[3][0] = clip[ 3] - clip[ 1];
		frustum[3][1] = clip[ 7] - clip[ 5];
		frustum[3][2] = clip[11] - clip[ 9];
		frustum[3][3] = clip[15] - clip[13];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[3][0] * frustum[3][0] + frustum[3][1] * frustum[3][1] + frustum[3][2] * frustum[3][2] );
		frustum[3][0] *= t;
		frustum[3][1] *= t;
		frustum[3][2] *= t;
		frustum[3][3] *= t;

		/* Extract the FAR plane */
		frustum[4][0] = clip[ 3] - clip[ 2];
		frustum[4][1] = clip[ 7] - clip[ 6];
		frustum[4][2] = clip[11] - clip[10];
		frustum[4][3] = clip[15] - clip[14];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[4][0] * frustum[4][0] + frustum[4][1] * frustum[4][1] + frustum[4][2] * frustum[4][2] );
		frustum[4][0] *= t;
		frustum[4][1] *= t;
		frustum[4][2] *= t;
		frustum[4][3] *= t;

		/* Extract the NEAR plane */
		frustum[5][0] = clip[ 3] + clip[ 2];
		frustum[5][1] = clip[ 7] + clip[ 6];
		frustum[5][2] = clip[11] + clip[10];
		frustum[5][3] = clip[15] + clip[14];

		/* Normalize the result */
		t = 1.f/sqrtf( frustum[5][0] * frustum[5][0] + frustum[5][1] * frustum[5][1] + frustum[5][2] * frustum[5][2] );
		frustum[5][0] *= t;
		frustum[5][1] *= t;
		frustum[5][2] *= t;
		frustum[5][3] *= t;

		IDSMatrix mm(4,4), mv(4,4);

		mm(0,0) = modl[0];
		mm(0,1) = modl[1];
		mm(0,2) = modl[2];
		mm(0,3) = modl[3];
		
		mm(1,0) = modl[4];
		mm(1,1) = modl[5];
		mm(1,2) = modl[6];
		mm(1,3) = modl[7];

		mm(2,0) = modl[8];
		mm(2,1) = modl[9];
		mm(2,2) = modl[10];
		mm(2,3) = modl[11];

		mm(3,0) = modl[12];
		mm(3,1) = modl[13];
		mm(3,2) = modl[14];
		mm(3,3) = modl[15];

		int i = mm.inv(&mv);

		viewerPos = IDSReal3D(mv(3,0),mv(3,1),mv(3,2));


		//IDSReal3D pos = IDSReal3D(-modl[12],-modl[13],-modl[14]);
		//IDSMatrix3D r;
		//r.setRows(IDSReal3D(modl[0],modl[1],modl[2]),
		//		  IDSReal3D(modl[4],modl[5],modl[6]),
		//	      IDSReal3D(modl[8],modl[9],modl[10]));
		//
		//IDSReal3D aaa = r*pos;
		//viewerPos = r*pos;
	}

	
	
	// -------------------------------------------------------------------------------------------------
	// TO BE DONE
	TestResult isBoxInFrustum(IDSBox &b)
	{
		TestResult result = INSIDE;
		//int out,in;

		//// for each plane
		//for(int i=0; i < 6; ++i)
		//{
		//	// reset counters for corners in and out
		//	out=0;in=0;

		//	// for each corner of the box
		//	for (int k = 0; k < 8 && (in==0 || out==0); k++)
		//	{
		//		// is the corner outside or inside
		//		if (planes[i].distance(b.corner(k)) < 0)
		//			out++;
		//		else
		//			in++;
		//	}
		//	//if all corners are out
		//	if (!in)
		//		return (OUTSIDE);
		//	// if some corners are out and others are in	
		//	else if (out)
		//		result = INTERSECT;
		//}
		return (result);

	// P-N Algorithm (requires a box with p-n corners)
	//int result = INSIDE;
	////for each plane
	//for(int i=0; i < 6; ++i)
	//{
	//	// is the positive vertex outside?
	//	if (planes[i].distance(b.getVertexP(pl[i].normal)) < 0)
	//		return OUTSIDE;
	//	// is the negative vertex outside?	
	//	else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
	//		result =  INTERSECT;
	//}
	//return(result);


	//	return OUTSIDE;
	}


	// -------------------------------------------------------------------------------------------------
	TestResult isSphereInFrustum(IDSReal3D pt, double radius)
	{
	   int c = 0;
	   double d;

	   for(int i = 0; i < 6; i++ )
	   {
		  d = frustum[i][0] * pt[0] + frustum[i][1] * pt[1] + frustum[i][2] * pt[2] + frustum[i][3];
		  if( d <= -radius )
			 return OUTSIDE;
		  if( d > radius )
			 c++;
	   }
	   return (c == 6) ? INSIDE : INTERSECT;
	}


	// -------------------------------------------------------------------------------------------------
	TestResult isPointInFrustum(IDSReal3D pt)
	{
		for(int i=0; i < 6; ++i)
			if( frustum[i][0] * pt[0] + frustum[i][1] * pt[1] + frustum[i][2] * pt[2] + frustum[i][3] <= 0 )
				return OUTSIDE;
		return INSIDE;
	}

	// -------------------------------------------------------------------------------------------------
	IDSReal3D  getViewerPos()    {return viewerPos;}
	
	// -------------------------------------------------------------------------------------------------
	int        getViewportSize() {return viewport[0]*viewport[1];}
};


}}
#endif FRUSTUM_H